#encoding=utf-8
import socket
import time
import numpy
import re
import serial 

host = "192.168.137.84"      #服务器ip
#查询本机IP命令
# $ hostname -I
#Formation 下
#一号机：192.168.137.84
#二号机:192.168.137.59
#HITSZ 下
#一号机：10.250.107.202
#二号机：10.250.207.224
#host = "127.0.0.1"      #服务器ip
#----------------如果端口被占用请在服务器和监听端的端口号一起改变即可---------------------
port = 6672               #服务器端口号
#====发送信息结构说明====
#[坐标x，'分隔符',坐标y，‘分隔符’，模式]
#坐标请按照UWB坐标进行传输
#模式一共1、2、3三种，1为起飞模式|2为定点飞行模式|3为降落模式
A = [2.13,',',2.14,',',1]
C = [2.13,',',2.14,',',3]
mySocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)    # 选择IPv4地址以及TCP协议
print ("通信服务开启")
mySocket.bind((host, port))   # 绑定端口
mySocket.listen(5)       # 监听这个端口，可连接最多5个设备
str1 = ''.join(str(i) for i in A)
mssg1 = str1.encode()
str3 = ''.join(str(i) for i in C)
mssg3 = str3.encode()

global count_get_uwb#只执行一次
global x
global y
count_get_uwb=0
x=0
y=0

def get_uwb():
    global count_get_uwb
    global x
    global y
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)#change tty/baud rate/timeout here
    while True:
        correct=0.5 # correction vlaue
        line = ser.readline()#read position info
        line=str(line)#byte->str
        if line:#if accepted
            a=re.search('KT',line)# find xyz information
            b=re.search('NULL',line)
            if a:
                if b:
                    print("ERROR. PLEASE CHECK UWB")
                if not b: 
                    count = line.split(',', 7) # standard
                    temp1=count[5].split('(')
                    temp2=count[7].split(')')
                    position=[0,0,0]
                    position[0]=temp1[1]
                    position[1]=count[6]
                    position[2]=temp2[0]
                    temp_x=x#上一轮次收到的位置信息
                    temp_y=y
                    x=float(position[1])#本轮收到的位置信息
                    y=float(position[0])
                    if count_get_uwb==0:
                        temp_x=x
                        temp_y=y
                        count_get_uwb=1
                    s1=x-temp_x#计算两次差值
                    s2=y-temp_y
                    if(abs(s1)>correct):#如果差值大于0.2进行修正
                        x=temp_x+np.sign(s1)*correct
                        print("Value [X] is corrected")
                    if(abs(s2)>correct):
                        y=temp_y+np.sign(s2)*correct
                        print("Value [Y] is corrected")
                    print("Camera position",x,y)
                    break
        else:
            break
    return


if __name__ == '__main__':
    client, address = mySocket.accept()           # 接受客户端的连接请求
    try:
        client.settimeout(120)                 # 设置2min时限
        client.send(mssg1)
        print("发送成功1")
        time.sleep(5)
    except socket.timeout:                # 超时
        print ('time out')
        
    for i in range(0,20):#循环次数可改变
        get_uwb()
        B = [x,',',y,',',2]
        str2 = ''.join(str(i) for i in B)
        mssg2 = str2.encode()
        try:
            client.settimeout(120)                 # 设置2min时限
            client.send(mssg2)
            print("发送成功2")
            time.sleep(0.5)
        except socket.timeout:                # 超时
            print ('time out')


    try:
        client.settimeout(120)                 # 设置2min时限
        client.send(mssg3)
        print("发送成功3")
        time.sleep(5)
    except socket.timeout:                # 超时
        print ('time out')
    mySocket.close()          # 关机

